// Ted Carancho's MikroQuad - February 2009
// www.MikroQuad.com
// An Arduino based quadrocopter using the Sparkfun
// 5DOF IMU and IDG300 Dual Axis Gyro
// This version will be able to use gyros for stability (acrobatic mode)
// or accelerometers (stable mode).

// This version is only compatible for the Arduino upgraded to the ATmega 328
// This can be obtained from: http://www.adafruit.com/index.php?main_page=product_info&cPath=17&products_id=123
// Additionally it must be uploaded with the Arduino 0011 IDE modified for the ATmega328
// This IDE can be downloaded from: http://spiffie.org/know/arduino_328/files/ide/arduino-m328-win_current.zip

// To Do:
// Create automated calibration for Z axis

#include <EEPROM.h>
#include <ServoTimer2.h>

// ******************** Initialize Variables ********************
#define BAUD 38400
#define LEDPIN 13

// Sensor pin assignments
/* Original orientation
#define XACCELPIN 0
#define YACCELPIN 1
#define ZACCELPIN 2
#define PITCHRATEPIN 3
#define ROLLRATEPIN 4
#define YAWRATEPIN 5*/
// New orientation
#define PITCHACCELPIN 4
#define ROLLACCELPIN 5
#define ZACCELPIN 3
#define PITCHRATEPIN 0
#define ROLLRATEPIN 1
#define YAWRATEPIN 2

// EEPROM storage
#define PGAIN_ADR 0
#define IGAIN_ADR 4
#define DGAIN_ADR 8
#define LEVEL_PGAIN_ADR 12
#define LEVEL_IGAIN_ADR 16
#define LEVEL_DGAIN_ADR 20
#define YAW_PGAIN_ADR 24
#define YAW_IGAIN_ADR 28
#define YAW_DGAIN_ADR 32
#define WINDUPGUARD_ADR 36
#define LEVELLIMIT_ADR 40
#define LEVELINTERVAL_ADR 44
#define XMITFACTOR_ADR 48
#define GYROSMOOTH_ADR 52
#define ACCSMOOTH_ADR 56
#define LEVELPITCHCAL_ADR 60
#define LEVELROLLCAL_ADR 64
#define FILTERTERM_ADR 68

// Transmitter variables
#define TIMEOUT 2500
#define THROTTLEPIN 4
#define ROLLPIN 2
#define PITCHPIN 3
#define YAWPIN 6
#define MODEPIN 7
#define AUXPIN 5
int throttle = 0;
int roll = 0;
int pitch = 0;
int yaw = 0;
int mode = 0; // >1500 stable mode,  <1500 acrobatic mode
int aux= 0;
int rollCommand = 1500;
int pitchCommand = 1500;
int yawCommand = 1500;
int rollCommandZero, pitchCommandZero, yawCommandZero;
// Controls the strength of the commands sent from the transmitter
// xmitFactor ranges from 0.01 - 1.0 (0.01 = weakest, 1.0 - strongest)
float xmitFactor; // Read in from EEPROM

// Motor control variables
#define MINCOMMAND 1000
#define MAXCOMMAND 2000
#define MINCHECK MINCOMMAND + 100
#define MAXCHECK MAXCOMMAND - 100
#define FRONTMOTORPIN 8
#define REARMOTORPIN 9
#define RIGHTMOTORPIN 10
#define LEFTMOTORPIN 11
ServoTimer2 frontMotor;
ServoTimer2 rearMotor;
ServoTimer2 rightMotor;
ServoTimer2 leftMotor;
int frontCommand;
int rearCommand;
int rightCommand;
int leftCommand;
int rollMotorCommand = 1500;
int pitchMotorCommand = 1500;
int yawMotorCommand = 1500;
// If AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
// Scale gyro output (-465 to +465) to motor commands (1000 to 2000)
// use y = mx + b
float mMotorRate = 1.0753; // m = (y2 - y1) / (x2 - x1) = (2000 - 1000) / (465 - (-465))
float bMotorRate = 1500;   // b = y1 - m * x1

// Accelerometer setup
// these A/D values depend on how well the sensors are mounted
// change these values to your unique configuration
// #define XMIN 405
// #define XMAX 607
// #define YMIN 409
// #define YMAX 618
#define ZMIN 403
#define ZMAX 611
#define ZEROLIMIT 2
int rollAccSensor, pitchAccSensor, zAccSensor;
int rollAccelZero, pitchAccelZero, zAdjust;
int rollAccADC, pitchAccADC, zAccADC;

// Auto level setup
int rollLevelAdjust = 0;
int pitchLevelAdjust = 0;
int levelLimit; // Read in from EEPROM
int levelInterval; // Read in from EEPROM

// Gyro setup
int rollGyroSensor, pitchGyroSensor, yawGyroSensor;
int rollGyroZero, pitchGyroZero, yawGyroZero;
int rollADC, pitchADC, yawADC;

// Complementary roll/pitch angle
float rollAngle, pitchAngle;
float filterTerm1, filterTerm2;
float timeConstant; // Read in from EEPROM

// Calibration parameters
#define FINDZERO 50
int findZero[FINDZERO];

// Low pass filter parameters
float gyroSmoothFactor, accSmoothFactor;

// PID Values
float windupGuard; // Read in from EEPROM
// Roll and pitch PID
float PID[3]; // Read in from EEPROM
float lastRollPosition = 0;
float iRollState = 0;
float lastPitchPosition = 0;
float iPitchState = 0;
// Level PID
float levelPID[3]; // Read in from EEPROM
float lastRollLevel = 0;
float iRollLevel = 0;
float lastPitchLevel = 0;
float iPitchLevel = 0;
// Yaw PID
float yawPID[3]; // Read in from EEPROM
float lastYawPosition = 0;
float iYawState = 0;

// Communication
char queryType = 'X';
char string[32];
byte armed = 0;

// Interrupt handler variables
byte timeSlot;

// Timing
long previousTime = 0;
long currentTime = 0;
long deltaTime = 0;
long enableAcc = 0;
float dt = 0;

// ******************** Setup MikroQuadAero ********************
void setup() {
  Serial.begin(BAUD);
  analogReference(EXTERNAL); // Current external ref is connected to 3.3V
  pinMode (LEDPIN, OUTPUT);
  digitalWrite(LEDPIN, LOW);
  
  // Configure motors
  frontMotor.attach(FRONTMOTORPIN);
  rearMotor.attach(REARMOTORPIN);
  rightMotor.attach(RIGHTMOTORPIN);
  leftMotor.attach(LEFTMOTORPIN);
  commandAllMotors(MINCOMMAND);

  // Read user values from EEPROM
  for (int i=0; i<3; i++) PID[i] = readFloat(PGAIN_ADR + (4 * i));
  for (int i=0; i<3; i++) levelPID[i] = readFloat(LEVEL_PGAIN_ADR + (4 * i));
  for (int i=0; i<3; i++) yawPID[i] = readFloat(YAW_PGAIN_ADR + (4 * i));
  windupGuard = readFloat(WINDUPGUARD_ADR);
  levelLimit = readFloat(LEVELLIMIT_ADR);
  levelInterval = readFloat(LEVELINTERVAL_ADR);
  xmitFactor = readFloat(XMITFACTOR_ADR);
  gyroSmoothFactor = readFloat(GYROSMOOTH_ADR);
  accSmoothFactor = readFloat(ACCSMOOTH_ADR);
  rollAccelZero = readFloat(LEVELROLLCAL_ADR);
  pitchAccelZero = readFloat(LEVELPITCHCAL_ADR);
  timeConstant = readFloat(FILTERTERM_ADR);

  // Setup interrupt to trigger on pin D2
  attachInterrupt(0, interruptHandler, RISING);
  
  // Wait for battery to be plugged in before ESC's are armed
  //armESC(3000);
  
  // Calibrate sensors
  zeroGyros();
  //zeroAccelerometers();
  zAdjust = ZMAX - ((ZMAX - ZMIN)/2);
  
  // Complementary filter setup
  filterTerm1 = timeConstant / (timeConstant + 0.010); //10ms = ESC update rate
  filterTerm2 = 1 - filterTerm1;
  
  digitalWrite(LEDPIN, HIGH);
  previousTime = millis();
}

// ******************** Main MikroQuad Loop ********************
void loop () {
  // Measure loop rate
  currentTime = millis();
  deltaTime = currentTime - previousTime;
  previousTime = currentTime;
  enableAcc++;
  
  // Send configuration commands from transmitter
  if (throttle < 1010) {
    zeroIntegralError();
    // Disarm motors (throttle down, yaw left)
    //if (yaw < MINCHECK && armed == 1) {
    if (yaw > MAXCHECK && armed == 1) {
      armed = 0;
      commandAllMotors(MINCOMMAND);
    }    
    // Zero sensors (throttle down, yaw left, roll left)
    //if (yaw < MINCHECK && roll > MAXCHECK) {
    if (yaw > MAXCHECK && roll < MINCHECK) {
      zeroGyros();
      zeroAccelerometers();
      zeroIntegralError();
      writeFloat(rollAccelZero, LEVELROLLCAL_ADR);
      writeFloat(pitchAccelZero, LEVELPITCHCAL_ADR);
      pulseMotors(3);
    }   
    // Arm motors (throttle down, yaw right)  
    //if (yaw > MAXCHECK && armed == 0) {
    if (yaw < MINCHECK && armed == 0) {
      armed = 1;
      pulseMotors(2);
      zeroIntegralError();
    }
  }

  // Read Sensors
  // Average gyro sensor values and center around zero
  rollADC = analogRead(ROLLRATEPIN) - rollGyroZero;
  pitchADC = analogRead(PITCHRATEPIN) - pitchGyroZero;
  yawADC = analogRead(YAWRATEPIN) - yawGyroZero;

  rollGyroSensor = smooth(rollADC, rollGyroSensor, gyroSmoothFactor);
  pitchGyroSensor = smooth(pitchADC, pitchGyroSensor, gyroSmoothFactor);
  yawGyroSensor = smooth(yawADC, yawGyroSensor, gyroSmoothFactor);

  rollAccADC = analogRead(ROLLACCELPIN) - rollAccelZero;
  pitchAccADC = analogRead(PITCHACCELPIN) - pitchAccelZero;
  zAccADC = analogRead(ZACCELPIN) - zAdjust;

  rollAccSensor = smooth(rollAccADC, rollAccSensor, accSmoothFactor);
  pitchAccSensor = smooth(pitchAccADC, pitchAccSensor, accSmoothFactor);
  zAccSensor = smooth(zAccADC, zAccSensor, accSmoothFactor);
  
  // accelerometerOutput = (N-512)/1024*(double)10.78;
  // gyroOutput = (N-512)/1024*(double)28.783;

  // if time constant = 250ms and control loop = 10ms, then 1st coeff = 0.9615, 2nd coeff = .0385
  dt = deltaTime / 1000.0;
  rollAngle = (filterTerm1 * (rollAngle + (rollADC / 1024.0 * 28.783 * dt))) + (filterTerm2 * (rollAccADC / 1024.0 * 10.78)) * 57.2957795;
  pitchAngle = (filterTerm1 * (pitchAngle + (pitchADC / 1024.0 * 28.783 * dt))) + (filterTerm2 * (pitchAccADC / 1024.0 * 10.78)) * 57.2957795;  
  
  // Check if time to read accelerometers, average, then center around zero
  if (mode < 1500) {
    // Acrobatic Mode
    rollLevelAdjust = 0;
    pitchLevelAdjust = 0;
  }
  else {
    // Stable Mode
    if ((enableAcc % levelInterval) == 0) {
      rollLevelAdjust = limitRange(updatePID(0, rollAngle, &lastRollLevel, &iRollLevel, levelPID), -levelLimit, levelLimit);
      pitchLevelAdjust = limitRange(updatePID(0, pitchAngle, &lastPitchLevel, &iPitchLevel, levelPID), -levelLimit, levelLimit);
    }
  }
  
  // Transmitter Commands
  // Calculations are performed in Interrupt.pde when receiver channels are read
  // Commands calculated are rollMotorCommand, pitchMotorCommand and yawMotorCommand
  // Saves processing time, since values only change when reading receiver
  
  // Update PID
  // Also convert sensor values to same range as PWM
  rollMotorCommand = updatePID(rollCommand - rollLevelAdjust, (mMotorRate * rollGyroSensor) + bMotorRate, &lastRollPosition, &iRollState, PID);
  pitchMotorCommand = updatePID(pitchCommand + pitchLevelAdjust, (mMotorRate * pitchGyroSensor) + bMotorRate, &lastPitchPosition, &iPitchState, PID);
  yawMotorCommand = updatePID(yawCommand, (mMotorRate * yawGyroSensor) + bMotorRate, &lastYawPosition, &iYawState, yawPID);   

  // Calculate motor commands
  // Original orientation
  // frontCommand =limitRange(throttle + pitchMotorCommand - yawMotorCommand - pitchLevelAdjust, MINCOMMAND, MAXCOMMAND);
  // rearCommand = limitRange(throttle - pitchMotorCommand - yawMotorCommand + pitchLevelAdjust, MINCOMMAND, MAXCOMMAND);
  // rightCommand = limitRange(throttle + rollMotorCommand + yawMotorCommand + rollLevelAdjust, MINCOMMAND, MAXCOMMAND);
  // leftCommand = limitRange(throttle - rollMotorCommand + yawMotorCommand - rollLevelAdjust, MINCOMMAND, MAXCOMMAND);
  if (armed) {
    frontCommand =limitRange(throttle + pitchMotorCommand + yawMotorCommand - pitchLevelAdjust, MINCOMMAND, MAXCOMMAND);
    rearCommand = limitRange(throttle - pitchMotorCommand + yawMotorCommand + pitchLevelAdjust, MINCOMMAND, MAXCOMMAND);
    rightCommand = limitRange(throttle - rollMotorCommand - yawMotorCommand - rollLevelAdjust, MINCOMMAND, MAXCOMMAND);
    leftCommand = limitRange(throttle + rollMotorCommand - yawMotorCommand + rollLevelAdjust, MINCOMMAND, MAXCOMMAND);
  }
  else {
    frontCommand = MINCOMMAND;
    rearCommand = MINCOMMAND;
    rightCommand = MINCOMMAND;
    leftCommand = MINCOMMAND;
  }
  
  // Command motors
  frontMotor.write(frontCommand);
  rearMotor.write(rearCommand);
  rightMotor.write(rightCommand);
  leftMotor.write(leftCommand);
    
  // Check for serial message
  if (Serial.available()) {
    queryType = Serial.read();
    if (queryType == 'C') {
      // Configure user defined values
      for (int i=0; i<3; i++) PID[i] = readFloatSerial();
      for (int i=0; i<3; i++) levelPID[i] = readFloatSerial();
      for (int i=0; i<3; i++) yawPID[i] = readFloatSerial();
      windupGuard = readFloatSerial();
      levelLimit = readFloatSerial();
      levelInterval = readFloatSerial();
      xmitFactor = readFloatSerial();
      gyroSmoothFactor = readFloatSerial();
      accSmoothFactor = readFloatSerial();
      timeConstant = readFloatSerial();
      queryType = 'X';
    }
    if (queryType == 'W') {
      for (int i=0; i<3; i++) writeFloat(PID[i], PGAIN_ADR + (4 * i));
      for (int i=0; i<3; i++) writeFloat(levelPID[i], LEVEL_PGAIN_ADR + (4 * i));
      for (int i=0; i<3; i++) writeFloat(yawPID[i], YAW_PGAIN_ADR + (4 * i));
      writeFloat(windupGuard, WINDUPGUARD_ADR);  
      writeFloat(levelLimit, LEVELLIMIT_ADR);   
      writeFloat(levelInterval, LEVELINTERVAL_ADR); 
      writeFloat(xmitFactor, XMITFACTOR_ADR);
      writeFloat(gyroSmoothFactor, GYROSMOOTH_ADR);
      writeFloat(accSmoothFactor, ACCSMOOTH_ADR);
      writeFloat(timeConstant, FILTERTERM_ADR);
      zeroIntegralError();
      
      // Complementary filter setup
      filterTerm1 = timeConstant / (timeConstant + 0.010); //10ms = ESC update rate
      filterTerm2 = 1 - filterTerm1;
    }
    // Other serial commands are handled by the interrupt handler (Interrupt.pde)
  }
}

// ******************** Subroutines ********************
// Used to read floating point values from the serial port
float readFloatSerial() {
  byte i = 0;
  byte timeout = 0;
  char data[128] = "";

  do {
    if (Serial.available() == 0) {
      delay(50);
      timeout++;
    }
    else {
      data[i++] = Serial.read();
      timeout = 0;
    }
  }  while ((data[limitRange(i-1, 0, 128)] != ';') && (timeout < 5) && (i < 128));
  
  return strtod(data, NULL);
}

// Sends commands to all motors
void commandAllMotors(int motorCommand) {
  frontMotor.write(motorCommand);
  rearMotor.write(motorCommand);
  rightMotor.write(motorCommand);
  leftMotor.write(motorCommand);
}

void pulseMotors(byte quantity) {
  for (byte i=0; i<quantity; i++) {      
    commandAllMotors(MINCOMMAND + 50);
    delay(200);
    commandAllMotors(MINCOMMAND);
    delay(200);
  }
}

/*
void armESC(unsigned long msWaitTime) {
  unsigned long startTime;
  startTime = millis();
  while ((millis() - startTime) < msWaitTime) {
    digitalWrite(FRONTMOTORPIN, HIGH);
    digitalWrite(REARMOTORPIN, HIGH);
    digitalWrite(RIGHTMOTORPIN, HIGH);
    digitalWrite(LEFTMOTORPIN, HIGH);
    delay(1);
    digitalWrite(FRONTMOTORPIN, LOW);
    digitalWrite(REARMOTORPIN, LOW);
    digitalWrite(RIGHTMOTORPIN, LOW);
    digitalWrite(LEFTMOTORPIN, LOW);
    delay(20);
  }
}
*/
